%%
%画出vision_pose的原始数据
subplot(2,2,3);
hold on;
plot(t_relative_vision, pose_vision_z);
ylabel('z');
subplot(2,2,1);
hold on;
plot(t_relative_vision, pose_vision_x);
ylabel('x');
subplot(2,2,2);
hold on;
plot(t_relative_vision, pose_vision_y);
ylabel('y');

%%
%画出dis_UWB的原始数据
subplot(3,1,3);
hold on;
plot(t_relative_UWB, dis_old(:,3));
plot(t_relative_vision, dis_test_3);
legend('UWB原始数据','参考距离')
ylabel('id3');
xlabel('t/s');
subplot(3,1,1);
hold on;
plot(t_relative_UWB, dis_old(:,1));
plot(t_relative_vision, dis_test_0);
legend('UWB原始数据','参考距离')
ylabel('id0');
xlabel('t/s');
subplot(3,1,2);
hold on;
plot(t_relative_UWB, dis_old(:,2));
plot(t_relative_vision, dis_test_1);
legend('UWB原始数据','参考距离')
ylabel('id1');
xlabel('t/s');
%%
%画出dis_sg滤波后的图像
subplot(2,2,3);
hold on;
plot(t_relative_UWB, dis_sg(:,3),'k');
plot(t_relative_UWB, dis_old(:,3));
legend('sg','origin');
ylabel('id3');
subplot(2,2,1);
hold on;
plot(t_relative_UWB, dis_sg(:,1),'k');
plot(t_relative_UWB, dis_old(:,1));
legend('sg','origin');
ylabel('id0');
subplot(2,2,2);
hold on;
plot(t_relative_UWB, dis_sg(:,2),'k');
plot(t_relative_UWB, dis_old(:,2));
legend('sg','origin');
ylabel('id1');
%%
%绘出xy图
clf;
p_start=4958/164*110;
p_end=4958/164*130;
plot(pose_vision_x(p_start:p_end), pose_vision_y(p_start:p_end));

%%
%绘出视觉与UWB的对比图
subplot(2,2,3);
hold on;
plot(t_relative_vision, pose_vision_z);
plot(t_relative_UWB, point_threeball(:,3));
ylabel('z');
subplot(2,2,1);
hold on;
plot(t_relative_vision, pose_vision_x);
plot(t_relative_UWB, point_threeball(:,1));
ylabel('x');
subplot(2,2,2);
hold on;
plot(t_relative_vision, pose_vision_y);
plot(t_relative_UWB, point_threeball(:,2));
ylabel('y');

%%
%绘出视觉与IMU的对比图
subplot(2,2,3);
hold on;
plot(t_relative_vision, pose_vision_z,'k');
% plot(t_relative_UWB, Pos_IMU(3,:));
plot(t_relative_UWB, Pos_IMU_sg(3,:));
ylabel('z');
subplot(2,2,1);
hold on;
plot(t_relative_vision, pose_vision_x,'k');
% plot(t_relative_UWB, Pos_IMU(1,:));
plot(t_relative_UWB, Pos_IMU_sg(1,:));
ylabel('x');
subplot(2,2,2);
hold on;
plot(t_relative_vision, pose_vision_y,'k');
% plot(t_relative_UWB, Pos_IMU(2,:));
plot(t_relative_UWB, Pos_IMU_sg(2,:));
ylabel('y');

%%
%各种比较图
hold on;
plot(t_relative_vision, pose_vision_y,'k');
plot(t_relative_UWB, Pos_IMU_sg(2,:));
ylabel('y');
xlabel('t/s');
% title('仅由IMU积分时的位置')
title('IMU积分+2个UWB（ID0+ID3）时的位置')
legend('Pose_{visionn}','Pose_{IMU}')